#include <Wire.h>
// Reference the HMC5883L Compass Library
#include <HMC5883L.h>
#include <Servo.h>
#include <TinyGPS.h>
#include <Wire.h> 
#include <String.h>
#include <Wstring.h>
#include <EEPROM.h>
#include <avr/eeprom.h>

// Store our compass as a variable.
HMC5883L compass;
// Record any errors that may occur in the compass.
int error = 0;

#define loiter 5000
#define deadzone 5
#define center 90
float headingDegrees;
#define T7 10000000
//#define compassdebug
//#define GPSdebug
TinyGPS gps;
int LEDflash = 0;
boolean HomePosStored = false;
int GPSFIX = 0;
int SATNUM = 0;
#define distanceError 0.00002
//#define highSpeed 64     
#define highSpeed 65     
#define lowSpeed 45
#define Headlights 5
float wayPoints[20];
float HomePoints[2];
float heading;
float falt, fc, fk;
float fmph = gps.f_speed_mph(); // speed in miles/hr
float flat, flon, x2lat, x2lon;
void printFloat(double f, int digits = 2);
Servo driveservo,steeringservo;                      
int noumWaypoints=0;
int Waycount=0;
float latitude, longitude, lastlat, lastlong;
float headingcompass;
int x4;
#define left 1300
#define right 1700
int RXCH[6];
int RXCHval[6];
int RXCHmin[6];
int RXCHmax[6];
volatile int RXSG[6];
int RXOK[6];
int PWMSG[6];


void setup()
{
  for(int j=0;j<7;j++){
    RXCHmin[j] = 255;    
    RXCHmax[j] = 255;
  }

  RXCH[1] = A0;  //Throttle
  RXCH[2] = 46;  //Aile / Yaw
  RXCH[3] = 44;  //Elev. / Pitch
  RXCH[4] = 42;  //Rudd. / Roll
  RXCH[5] = A4;  //Gear
  RXCH[6] = A5;  //Aux / Flt Mode
  for (int i = 1; i < 7; i++){ 
    pinMode(RXCH[i], INPUT);
  }

  Serial.begin(115200);               //SETUP SERIAL MONITOR AND GPS
  Serial1.begin(4800);
  delay(1000);
  //  initcompass();
  Wire.begin(); // Start the I2C interface.

  Serial.println("Constructing new HMC5883L");
  compass = HMC5883L(); // Construct a new HMC5883 compass.

  Serial.println("Setting scale to +/- 1.3 Ga");
  error = compass.SetScale(1.3); // Set the scale of the compass.
  if(error != 0) // If there is an error, print it out.
    Serial.println(compass.GetErrorText(error));

  Serial.println("Setting measurement mode to continous.");
  error = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous
  if(error != 0) // If there is an error, print it out.
    Serial.println(compass.GetErrorText(error));

  driveservo.attach(9,1000,2000);       //ATTACH STEERING SERVO
  steeringservo.attach(6,1000,2000);           //ATTACH SERVO FOR THE SENSOR
  driveservo.writeMicroseconds(1500);   //GO TO CENTER
  steeringservo.writeMicroseconds(1500);   //GO TO CENTER  

  wayPoints[0] = 51.4288203415286;
  wayPoints[1] = 0.555048286914825;
  wayPoints[2] = 51.4288947597462;
  wayPoints[3] = 0.554912835359573;
  wayPoints[4] = 51.4289457654208;
  wayPoints[5] = 0.555022805929184;
  wayPoints[6] = 51.4288863981547;
  wayPoints[7] = 0.555159598588943;

  noumWaypoints = 8;
  delay(1000);
  pinMode(Headlights,OUTPUT);
  digitalWrite(Headlights,HIGH);
  //  RXCHval[3] = 0;
  //  RXCHval[4] = 0;
}
boolean btnlaststate = false;
static boolean LEDstate = LOW;
static boolean LED2state = LOW;
boolean clearWayPoints = false;

void loop()
{
  readGps();
  //  readcomp();
#ifdef compassdebug
  Serial.print("Heading=");
  Serial.println(getAngleInDeg());
#endif
  if (SATNUM > 1){
    Serial.println("*** GPS FIX ***");
    //    digitalWrite(BlueLED,HIGH);
  }
  else{
    Serial.println("*** NO GPS ***");
    //    digitalWrite(BlueLED,LOW); 
  }
  // Read RX values
  for (int i = 1; i < 5; i++){						    //for each of the 6 channels:
    RXSG[i] = pulseIn(RXCH[i], 254, 20000);				    //read the receiver signal
    if (RXSG[i] == 0) {
      RXSG[i] = RXOK[i];
    } 
    else {
      RXOK[i] = RXSG[i];
    }    //if the signal is good then use it, else use the previous signal
    PWMSG[i] = map(RXSG[i], 1000, 2000, 0, 511);			//substitute the 254 values to a value between 0 and 511
    constrain (PWMSG[i], 0, 511);	
    RXCHval[i] = PWMSG[i];

    if (PWMSG[i] < RXCHmin[i]){
      RXCHmin[i] = PWMSG[i];
    }
    if (PWMSG[i] > RXCHmax[i]){
      RXCHmax[i] = PWMSG[i];
    }
    delay(5);
  }
  //  Serial.print("RCCH4: ");
  //  Serial.println(RXCHval[4]);
  int chan2 = constrain(map(RXCHval[2],RXCHmin[2],RXCHmax[2],0,180),0,180);
  int chan3 = constrain(map(RXCHval[3],RXCHmin[3],RXCHmax[3],0,180),0,180);
  if (chan2 > center - deadzone && chan2 < center + deadzone){
    chan2 = center; 
  }
  if (chan3 > center - deadzone && chan3 < center + deadzone){
    chan3 = center; 
  }
  //Serial << "Gear Reading: " << RXCHval[3] <<endl;
  if (RXCHval[4] > RXCHmax[4] / 2){
    Serial.println("MANUAL MODE!");
    Serial.print("CH3:");
    Serial.print(chan3);
    Serial.print("\tCH2:");
    Serial.println(chan2);

    driveservo.write(chan3);   
    steeringservo.write(chan2);   

  }
  else{
    Serial.println("AUTO MODE!");
    getAngleInDeg();
    ReadWay();
  }
  //  delay(100);
}
int currentway=0;
float currentlat;
float currentlon;

void ReadWay(){
  if (noumWaypoints > 0 && SATNUM > 0){
    if(currentway == noumWaypoints){
      motorStop();
      //      delay(5000); 
    }
    else{
      currentlat = wayPoints[currentway];
      currentlon = wayPoints[currentway+1];

      Navigate(currentlat,currentlon);

      if(((currentlon-longitude)<distanceError && (currentlon-longitude)>-distanceError) && ((currentlat-latitude)<distanceError && (currentlat-latitude)>-distanceError))
      {
        motorStop();
        delay(loiter);
        currentway ++;  
        currentway ++;  
      }
    }
  }

}


void Navigate(float cflat,float cflon  ){

  float flat1 =latitude; 
  float flon1 =longitude;
  Serial.print("Latitude: ");
  Serial.print(longitude,DEC);
  Serial.print("\tLongitude: ");
  Serial.println(flon1,DEC);
  float dist_calc=0;
  float dist_calc2=0;
  float diflat=0;
  float diflon=0;
  x2lat=cflat;  
  x2lon=cflon;
  //Calculate distance from current location to waypoint
  diflat=radians(x2lat-flat1);  //Must be done in radians
  flat1=radians(flat1);    //convert current latitude to radians
  x2lat=radians(x2lat);  //convert waypoint latitude to radians
  diflon=radians((x2lon)-(flon1));   //subtract and convert longitudes to radians
  dist_calc = (sin(diflat/2.0)*sin(diflat/2.0));
  dist_calc2= cos(flat1);
  dist_calc2*=cos(x2lat);
  dist_calc2*=sin(diflon/2.0);                                       
  dist_calc2*=sin(diflon/2.0);
  dist_calc +=dist_calc2;
  dist_calc=(2*atan2(sqrt(dist_calc),sqrt(1.0-dist_calc)));
  dist_calc*=6371000.0; //Converting to meters
  Serial.print("Distance to waypoint: ");
  Serial.print(dist_calc);    //print the distance in meters
  flon1 = radians(flon1);  //also must be done in radians
  x2lon = radians(x2lon);  //also must be done in radians
  heading = atan2(sin(x2lon-flon1)*cos(x2lat),cos(flat1)*sin(x2lat)-sin(flat1)*cos(x2lat)*cos(x2lon-flon1)),2*3.1415926535;
  heading = heading*180/3.1415926535;  // convert from radians to degrees
  int head =heading; //make it a integer now
  if(head<0){
    heading+=360;   //if the heading is negative then add 360 to make it positive
  }
  Serial.print("\theading:");
  Serial.print(heading);   // print the heading.
  int newheading = headingcompass;// +180;
  x4=newheading-heading;   //getting the difference of our current heading to our needed bearing
  Serial.print("\tHeading Difference: ");
  Serial.println(x4);
  int turn;
  if(x4>=180){
    if(x4<=360){
      Serial.print("Turning Right at speed: ");
      Serial.println(lowSpeed);
      motorRight(lowSpeed);
    }
  }
  if(x4>=0){
    if(x4<180){
      Serial.print("Turning Left at speed: ");
      Serial.println(lowSpeed);
      motorLeft(lowSpeed); 
    }
  }

   if(x4 < 10 && x4 > 0 || x4 > 350 && x4 < 360){
    if(dist_calc > 15){
      Serial.print("Go Straight FAST at speed: ");
      Serial.println(highSpeed);
      motorForward(highSpeed);
    }
    else{
      Serial.print("Go Straight SLOW at speed:");
      Serial.println(lowSpeed);      
      motorForward(lowSpeed);   //go "straight"
    }
  }
}

void readGps(){

  bool newData = false;
  while (Serial1.available())
  {
    char c = Serial1.read();
    // Serial.write(c); // uncomment this line if you want to see the GPS data flowing
    if (gps.encode(c)) // Did a new valid sentence come in?
      newData = true;
  }
  if (newData)
  {
    Serial.println("GPS DATA FOUND");
    falt = gps.f_altitude(); // +/- altitude in meters
    fc = gps.f_course(); // course in degrees
    fk = gps.f_speed_knots(); // speed in knots
    fmph = gps.f_speed_mph(); // speed in miles/hr
    unsigned long age;
    gps.f_get_position(&flat, &flon, &age);
    flat == TinyGPS::GPS_INVALID_F_ANGLE ? 0.0 : flat;
    flon == TinyGPS::GPS_INVALID_F_ANGLE ? 0.0 : flon;
    gps.satellites() == TinyGPS::GPS_INVALID_SATELLITES ? 0 : gps.satellites();
    gps.hdop() == TinyGPS::GPS_INVALID_HDOP ? 0 : gps.hdop();

    lastlat = latitude;
    lastlong = longitude;
    latitude =(float)flat;///T7;
    longitude = (float)flon;///T7;
    SATNUM = (int)gps.satellites();
    if(SATNUM > 0){
      if(!HomePosStored){
        HomePoints[0] = latitude;
        HomePoints[1] = longitude;
        HomePosStored = true;
      }
    }
  }

#ifdef GPSdebug
  Serial.print("LAT=");
  Serial.print(flat, 6);
  Serial.print("\tLON=");
  Serial.print(flon, 6);
  Serial.print("\tSAT=");
  Serial.print(gps.satellites());
  Serial.print("\tPREC=");
  Serial.print(gps.hdop());
  Serial.print("\tALT= ");
  Serial.print(falt);
  Serial.print("\tCourse= ");
  Serial.println(fc);

#endif 
}









